#include <SoftwareSerial.h>
 
const int DEFAULT_BAUD = 9600;
const int SERVO_CONTROLLER_RX_PIN = 11; // The SERVO CONTROLLER'S RX PIN.
const int SERVO_CONTROLLER_TX_PIN = 10; // The SERVO CONTROLLER'S TX PIN.

SoftwareSerial ServoController = SoftwareSerial(SERVO_CONTROLLER_RX_PIN, SERVO_CONTROLLER_TX_PIN);

void setup()
{
   ServoController.begin(DEFAULT_BAUD);
   delay(500);
}

void loop()
{
   moveServo(1, 0); //Move the servo on channel 0 to an angle of 0 degrees
   moveServo(0, 0); //Move the servo on channel 1 to an angle of 0 degrees
   delay(2000); //Wait 2000ms
   moveServo(1, 180); //Move the servo on channel 0 to an angle of 180 degrees
   moveServo(0, 180); //Move the servo on channel 1 to an angle of 180 degrees
   delay(2000); //Wait 2000ms
}


void moveServo(int ServoChannel, int target)
{
   //656ms PWM pulse represents a servo angle of 0 degrees.
   //2000ms PWM pulse represents a servo angele of 180 degrees.
   //These values could vary based on the servo you use, check your servo's 
   //spec documentation and verify what PWM pulses are needed to move it.
 
   byte serialBytes[4]; //Create the byte array object that will hold the communication packet.
 
   target = (map(target, 0, 180, 656, 2000) * 4); //Map the target angle to the corresponding PWM pulse.
 
   serialBytes[0] = 0x84; // Command byte: Set Target.
   serialBytes[1] = ServoChannel; // First byte holds channel number.
   serialBytes[2] = target & 0x7F; // Second byte holds the lower 7 bits of target.
   serialBytes[3] = (target >> 7) & 0x7F; // Third byte holds the bits 7-13 of target.
 
   ServoController.write(serialBytes, sizeof(serialBytes)); //Write the byte array to the serial port.
}
